clc;
% 加载数据
% measure_data  ： [测量位置     测量速度]
% real_sate     ： [实际位置     实际速度]
load('../data/measure_data.mat')
load('../data/real_state.mat')


% 绘制实际状态曲线
figure('name','kalman filter example');
subplot(2,1,1)
plot(real_state(1,:),'blue');   % 绘制实际位置曲线
title('位置曲线');
hold on;

subplot(2,1,2)
plot(real_state(2,:),'blue');   % 绘制实际速度曲线
title('速度曲线');
hold on;

% 绘制量测数据曲线
subplot(2,1,1)
plot(measure_data(1,:),'g');   %绘制量测位置
subplot(2,1,2)
plot(measure_data(2,:),'g');   %绘制量测速度

% 设置滤波器参数
A = [1,1;
     0,1];
 
H = [1,0;
     0,1];
 
Q = [0.2,0;
     0,0.2];    %状态噪声协方差
 
R = [1,0;
     0,1];      %测量噪声协方差
G = [1,0;
     0,1];
T = 1;  %采样时间设为1s

kf = kalman_filter(A,H,Q,R,G,T);    % 初始化滤波器

X_apr = zeros(2,32);
X_est = zeros(2,32);

% 设置初始参数
X = [0;
     1];
P = [1,0;
     0,1];
% 开始滤波

for i = 1:30
% 预测更新
    [X,P_apr] = kf.prediction(X,P);
    X_apr(:,i+1) = X;
    
% 校正
    [X, P] = kf.update(X_apr(:,i+1),P_apr,measure_data(:,i+1));
    X_est(:,i + 1) = X;
end

% 绘制先验数据
subplot(2,1,1)
plot(X_apr(1,1:end-1),'c');   %绘制先验位置
subplot(2,1,2)
plot(X_apr(2,1:end-1),'c');   %绘制先验速度

% 绘制后验数据
subplot(2,1,1)
plot(X_est(1,1:end-1),'m');   %绘制后验位置
legend('实际位置','量测位置','先验位置','后验位置');
subplot(2,1,2)
plot(X_est(2,1:end-1),'m');   %绘制后验速度
legend('实际速度','量测速度','先验速度','后验速度');








